package NavMesh

import (
	"fmt"
	"io/ioutil"
	. "misc/NavMesh/Detour"
	"os"
	. "utils"
)

type NavMesh struct {
	RawTileData *DTRawTileData
	DTNav       *DTNavMesh
	DTQuery     *DTNavMeshQuery
}

func (this *NavMesh) LoadNavMesh(path string) {
	f, err := os.Open(path)
	if err != nil {
		fmt.Println(err)
	}
	defer f.Close()
	allbytes, err2 := ioutil.ReadAll(f)
	if err2 != nil {
		fmt.Println(err2)
	}

	this.RawTileData = &DTRawTileData{}
	this.RawTileData.FromBytes(allbytes, 0)

	this.DTNav = NewDTNavMesh()
	this.DTNav.InitByRawData(this.RawTileData, DT_TILE_FREE_DATA)
	this.DTQuery = NewDTNavMeshQuery()
	this.DTQuery.Init(this.DTNav, 2048)
}

func (this *NavMesh) FindPath(startPos, endPos Vec3) []Vec3 {
	extents := make([]float64, 3)
	extents[0], extents[1], extents[2] = 10, 10, 10
	var startRef, endRef DTPolyRef
	startPt := make([]float64, 3)
	endPt := make([]float64, 3)
	filter := NewDTQueryFilter()
	this.DTQuery.FindNearestPoly(startPos.ToArray(), extents, filter, &startRef, startPt)
	this.DTQuery.FindNearestPoly(endPos.ToArray(), extents, filter, &endRef, endPt)

	pathCount := -1
	polys := make([]DTPolyRef, 256)
	this.DTQuery.FindPath(startRef, endRef, startPt, endPt, filter, polys, &pathCount, 256)
	if pathCount > 0 {
		epos := make([]float64, 3)
		DTVcopy(epos, endPt)
		if polys[pathCount-1] != endRef {
			posOverPoly := false
			this.DTQuery.ClosestPointOnPoly(polys[pathCount-1], endPt, epos, &posOverPoly)
		}
		_, lstVec := this.findStraightPath(startPt, endPt, polys, pathCount)
		return lstVec
	}
	return nil
}

func (this *NavMesh) findStraightPath(startPos, endPos []float64, path []DTPolyRef, pathSize int) (DTStatus, []Vec3) {
	lstVec := make([]Vec3, 0)
	closestStartPos := make([]float64, 3)
	if DTStatusFailed(this.DTQuery.ClosestPointOnPolyBoundary(path[0], startPos, closestStartPos)) {
		return DT_FAILURE | DT_INVALID_PARAM, nil
	}
	closestEndPos := make([]float64, 3)
	if DTStatusFailed(this.DTQuery.ClosestPointOnPolyBoundary(path[pathSize-1], endPos, closestEndPos)) {
		return DT_FAILURE | DT_INVALID_PARAM, nil
	}

	lstVec = append(lstVec, Vec3{startPos[0], startPos[1], startPos[2]})
	if pathSize > 1 {
		portalApex := make([]float64, 3)
		portalLeft := make([]float64, 3)
		portalRight := make([]float64, 3)
		DTVcopy(portalApex, closestStartPos)
		DTVcopy(portalLeft, portalApex)
		DTVcopy(portalRight, portalApex)
		var apexIndex, leftIndex, rightIndex int

		for i := 0; i < pathSize; i++ {
			left := make([]float64, 3)
			right := make([]float64, 3)
			var fromType, toType byte

			if i+1 < pathSize {
				// Next portal.
				if DTStatusFailed(this.DTQuery.GetPortalPoints(path[i], path[i+1], left, right, &fromType, &toType)) {

					if DTStatusFailed(this.DTQuery.ClosestPointOnPolyBoundary(path[i], endPos, closestEndPos)) {
						return DT_FAILURE | DT_INVALID_PARAM, nil
					}
					lstVec[i] = Vec3{closestEndPos[0], closestEndPos[1], closestEndPos[2]}
					return DT_SUCCESS, lstVec
				}

				if i == 0 {
					var t float64
					if DTDistancePtSegSqr2D(portalApex, 0, left, 0, right, 0, &t) < DTSqr(0.001) {
						continue
					}
				}
			} else {
				DTVcopy(left, closestEndPos)
				DTVcopy(right, closestEndPos)
				toType = DT_POLYTYPE_GROUND
				fromType = DT_POLYTYPE_GROUND
			}

			// Right vertex.
			if DTTriArea2D(portalApex, portalRight, right) <= 0.0 {
				if DTVequal(portalApex, portalRight) || DTTriArea2D(portalApex, portalLeft, right) > 0.0 {
					DTVcopy(portalRight, right)

					rightIndex = i
				} else {
					DTVcopy(portalApex, portalLeft)
					apexIndex = leftIndex
					lstVec = append(lstVec, Vec3{portalApex[0], portalApex[1], portalApex[2]})
					DTVcopy(portalLeft, portalApex)
					DTVcopy(portalRight, portalApex)
					leftIndex = apexIndex
					rightIndex = apexIndex
					i = apexIndex
					continue
				}
			}

			// Left vertex.
			if DTTriArea2D(portalApex, portalLeft, left) >= 0.0 {
				if DTVequal(portalApex, portalLeft) || DTTriArea2D(portalApex, portalRight, left) < 0.0 {
					DTVcopy(portalLeft, left)

					leftIndex = i
				} else {
					DTVcopy(portalApex, portalRight)
					apexIndex = rightIndex
					lstVec = append(lstVec, Vec3{portalApex[0], portalApex[1], portalApex[2]})
					DTVcopy(portalLeft, portalApex)
					DTVcopy(portalRight, portalApex)
					leftIndex = apexIndex
					rightIndex = apexIndex
					i = apexIndex
					continue
				}
			}
		}
	}
	lstVec = append(lstVec, Vec3{closestEndPos[0], closestEndPos[1], closestEndPos[2]})
	return DT_SUCCESS, lstVec
}
